//
//  SAMatrixUtil.c
//  GameFrame
//
//  Created by Hiroshi Nomura on 2014/05/01.
//  Copyright (c) 2014年 Hiroshi Nomura. All rights reserved.
//

#include <stdio.h>
#include <math.h>

#include "SAVectorUtil.h"
#include "SAMatrixUtil.h"

void populatePerspectiveMatrix4x4(float fovy, float aspect, float near, float far, Matrix4x4 mtx)
{
    float f = 1.0f / tanf( (fovy * (M_PI/180)) / 2.0f);
	
	mtx[0] = f / aspect;
	mtx[1] = 0.0f;
	mtx[2] = 0.0f;
	mtx[3] = 0.0f;
	
	mtx[4] = 0.0f;
	mtx[5] = f;
	mtx[6] = 0.0f;
	mtx[7] = 0.0f;
	
	mtx[8] = 0.0f;
	mtx[9] = 0.0f;
	mtx[10] = (far+near) / (near-far);
	mtx[11] = -1.0f;
	
	mtx[12] = 0.0f;
	mtx[13] = 0.0f;
	mtx[14] = 2 * far * near /  (near-far);
	mtx[15] = 0.0f;

}

void populateIdentityMatrix4x4(Matrix4x4 mtx)
{
	mtx[0] = mtx[5] = mtx[10] = mtx[15] = 1.0f;
    
	mtx[1] = mtx[2] = mtx[3] = mtx[4] =
	mtx[6] = mtx[7] = mtx[8] = mtx[9] =
	mtx[11] = mtx[12] = mtx[13] = mtx[14] = 0.0;
}

void inverseMatrix4x4(Matrix4x4 mtx)
{
    
}

void transposeMatrix4x4(Matrix4x4 src_mtx, Matrix4x4 mtx)
{
    // [ 0 4  8 12 ]
	// [ 1 5  9 13 ]
	// [ 2 6 10 14 ]
    // [ 3 7 11 15 ]
    
	float tmp;
	mtx[0]  = src_mtx[0];
    
	tmp = src_mtx[4];
	mtx[4]  = src_mtx[1];
	mtx[1]  = tmp;

    tmp = src_mtx[8];
	mtx[8]  = src_mtx[2];
	mtx[2] = tmp;

    tmp = src_mtx[12];
	mtx[12] = src_mtx[3];
	mtx[3]  = tmp;
    
	mtx[5]  = src_mtx[5];

    tmp = src_mtx[9];
	mtx[9]  = src_mtx[6];
	mtx[6]  = tmp;

    tmp = src_mtx[13];
	mtx[13] = src_mtx[7];
	mtx[7]  = tmp;
    
    mtx[10] = src_mtx[10];
    
    tmp = src_mtx[14];
	mtx[14] = src_mtx[11];
	mtx[11] = tmp;
    
	mtx[15] = src_mtx[15];
}

void translateMatrix4x4(Matrix4x4 mtx, float x, float y, float z)
{
    mtx[12] += mtx[0]*x + mtx[4]*y + mtx[8]*z;
	mtx[13] += mtx[1]*x + mtx[5]*y + mtx[9]*z;
	mtx[14] += mtx[2]*x + mtx[6]*y + mtx[10]*z;
}

void scaleMatrix4x4(Matrix4x4 mtx, float x, float y, float z)
{
    mtx[0]  *= x;
	mtx[1]  *= x;
	mtx[2]  *= x;
	mtx[3]  *= x;
	
    mtx[4]  *= y;
	mtx[5]  *= y;
    mtx[6]  *= y;
	mtx[7]  *= y;
    
	mtx[8]  *= z;
    mtx[9]  *= z;
	mtx[10] *= z;
	mtx[11] *= z;
}

void rotateXMatrix4x4(Matrix4x4 mtx, float rad)
{
    
}

void rotateYMatrix4x4(Matrix4x4 mtx, float rad)
{
    
}

void rotateZMatrix4x4(Matrix4x4 mtx, float rad)
{
    
}

void multiplyMatrix4x4ByMatrix4x4(Matrix4x4 lmtx, Matrix4x4 rmtx, Matrix4x4 mtx)
{
    mtx[ 0] = lmtx[ 0]*rmtx[ 0] + lmtx[ 4]*rmtx[ 1] + lmtx[ 8]*rmtx[ 2] + lmtx[12]*rmtx[ 3];
	mtx[ 1] = lmtx[ 1]*rmtx[ 0] + lmtx[ 5]*rmtx[ 1] + lmtx[ 9]*rmtx[ 2] + lmtx[13]*rmtx[ 3];
	mtx[ 2] = lmtx[ 2]*rmtx[ 0] + lmtx[ 6]*rmtx[ 1] + lmtx[10]*rmtx[ 2] + lmtx[14]*rmtx[ 3];
	mtx[ 3] = lmtx[ 3]*rmtx[ 0] + lmtx[ 7]*rmtx[ 1] + lmtx[11]*rmtx[ 2] + lmtx[15]*rmtx[ 3];
    
	mtx[ 4] = lmtx[ 0]*rmtx[ 4] + lmtx[ 4]*rmtx[ 5] + lmtx[ 8]*rmtx[ 6] + lmtx[12]*rmtx[ 7];
	mtx[ 5] = lmtx[ 1]*rmtx[ 4] + lmtx[ 5]*rmtx[ 5] + lmtx[ 9]*rmtx[ 6] + lmtx[13]*rmtx[ 7];
	mtx[ 6] = lmtx[ 2]*rmtx[ 4] + lmtx[ 6]*rmtx[ 5] + lmtx[10]*rmtx[ 6] + lmtx[14]*rmtx[ 7];
	mtx[ 7] = lmtx[ 3]*rmtx[ 4] + lmtx[ 7]*rmtx[ 5] + lmtx[11]*rmtx[ 6] + lmtx[15]*rmtx[ 7];
    
	mtx[ 8] = lmtx[ 0]*rmtx[ 8] + lmtx[ 4]*rmtx[ 9] + lmtx[ 8]*rmtx[10] + lmtx[12]*rmtx[11];
	mtx[ 9] = lmtx[ 1]*rmtx[ 8] + lmtx[ 5]*rmtx[ 9] + lmtx[ 9]*rmtx[10] + lmtx[13]*rmtx[11];
	mtx[10] = lmtx[ 2]*rmtx[ 8] + lmtx[ 6]*rmtx[ 9] + lmtx[10]*rmtx[10] + lmtx[14]*rmtx[11];
	mtx[11] = lmtx[ 3]*rmtx[ 8] + lmtx[ 7]*rmtx[ 9] + lmtx[11]*rmtx[10] + lmtx[15]*rmtx[11];
    
	mtx[12] = lmtx[ 0]*rmtx[12] + lmtx[ 4]*rmtx[13] + lmtx[ 8]*rmtx[14] + lmtx[12]*rmtx[15];
	mtx[13] = lmtx[ 1]*rmtx[12] + lmtx[ 5]*rmtx[13] + lmtx[ 9]*rmtx[14] + lmtx[13]*rmtx[15];
	mtx[14] = lmtx[ 2]*rmtx[12] + lmtx[ 6]*rmtx[13] + lmtx[10]*rmtx[14] + lmtx[14]*rmtx[15];
	mtx[15] = lmtx[ 3]*rmtx[12] + lmtx[ 7]*rmtx[13] + lmtx[11]*rmtx[14] + lmtx[15]*rmtx[15];
}

void multiplyMatrix4x4ByVector4D(Matrix4x4 lmtx, Vector4D rvec)
{
    
}

void multiplyQuaternionByQuaternion(Vector4D lvec, Vector4D rvec, Vector4D vec)
{
    /*GLfloat tV[3];
	GLfloat sV[3];
	tV[0]=tQ->x; tV[1]=tQ->y; tV[2]=tQ->z;
	sV[0]=sQ->x; sV[1]=sQ->y; sV[2]=sQ->z;
	
	GLfloat aV[3], bV[3];
	aV[0]=tQ->w*sV[0]; aV[1]=tQ->w*sV[1]; aV[2]=tQ->w*sV[2];
	bV[0]=sQ->w*tV[0]; bV[1]=sQ->w*tV[1]; bV[2]=sQ->w*tV[2];
	GLfloat cV[3];
	vec3CrossProduct(cV, tV, sV);
	
	vec->x = aV[0] + bV[0] + cV[0];
	vec->y = aV[1] + bV[1] + cV[1];
	vec->z = aV[2] + bV[2] + cV[2];
	vec->w = tQ->w*sQ->w - vec3DotProduct(tV, sV);*/
}

void convertMatrix4x4ToQuaternion(Matrix4x4 mtx, Vector4D vec)
{
    
}

void convertQuaternionToMatrix4x4(Vector4D vec, Matrix4x4 mtx)
{
    
}
